/*
 *      stone.cpp
 *      
 *      Copyright 2008 Antti Leivo <skylark@skylark-desktop>
 *      
 *      This program is free software; you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License as published by
 *      the Free Software Foundation; either version 2 of the License, or
 *      (at your option) any later version.
 *      
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *      
 *      You should have received a copy of the GNU General Public License
 *      along with this program; if not, write to the Free Software
 *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 *      MA 02110-1301, USA.
 */

#include <cmath>
#include "world.h"

World::World(void)
{
	/* chipmunk initialization */
	cpInitChipmunk();
	if ((space = cpSpaceNew()) == NULL)
	{
		status = sysFailure;
		return;
	}
    space->iterations = 10;
    space->elasticIterations = 10;
    space->gravity = cpvzero;
	status = sysRunnable;
}

World::~World()
{
	if (space != NULL)
	{
		cpSpaceFree(space);
	}
}

void World::step(float _time)
{

	for(std::vector<Stone*>::iterator i = stones.begin(); i != stones.end(); i++)
	{
		/* Chipmunks based apply gravity force */
		Stone& si = **i;
		cpBodyResetForces(si.body);

		for(std::vector<Stone*>::iterator j = stones.begin(); j != stones.end(); j++)
		{
			if (i != j)
			{
				Stone& sj = **j;
				float _dist = sqrt((si.body->p.x-sj.body->p.x)*(si.body->p.x-sj.body->p.x)
				                  +(si.body->p.y-sj.body->p.y)*(si.body->p.y-sj.body->p.y));
				float F = WORLD_GRAVITY_CONSTANT*si.body->m*sj.body->m/(_dist*_dist);
				float force_x = F * (sj.body->p.x - si.body->p.x) / _dist;
				float force_y = F * (sj.body->p.y - si.body->p.y) / _dist;
				/* Chipmunks apply force */
				cpBodyApplyForce(si.body, /*F vector*/cpv(force_x, force_y), /*force place offset*/cpvzero);
			}
		} // for j
	} // for i

	/* Chipmunks base solver step for entire space */
	cpSpaceStep(space, _time);
	return;

	/* old step(). commented out */

}

void World::insertStone(Stone *s)
{
	if (status == sysRunnable)
	{
		stones.push_back(s);
		s->attachCpSpace(space);
	}
}

ESysStatus World::getStatus()
{
	return status;
}


